#include "point_operator.h"
#include <assert.h>
#include <math.h>

point_operator::point_operator(){}

void point_operator::AddTwoPoints(Point A[3], Point B[3], Point point[3])
{
	for(int i=0; i<3; i++)
		point[i] = A[i] + B[i];
}

void point_operator::SubtractTwoPoints(Point A[3], Point B[3], Point point[3])
{
	for(int i=0; i<3; i++)
		point[i] = A[i] - B[i];
}
Point point_operator::DotProduct(Point* A, Point* B)
{
	return (A[0]*B[0] + A[1]*B[1] + A[2]*B[2]);
}
void point_operator::CrossProduct(Point A[3], Point B[3], Point point[3])
{
	point[0] = A[1]*B[2] - B[1]*A[2];
	point[1] = -(A[0]*B[2] - A[2]*B[0]);
	point[2] = A[0]*B[1] - A[1]*B[0];
}
void point_operator::SetPointEqualToPoint(Point A[3], Point point[3])
{
	for(int i=0; i<3; i++)
		point[i] = A[i];
}
bool point_operator::AreTwoPointsEqual(Point A[3], Point B[3])
{
	/*
	for(int i=0; i<3; i++)
		if(A[i] != B[i])
			return false;
			*/

	for(int i=0; i<3; i++)
		if( !AlmostEqual2sComplement(A[i], B[i], 1))
			return false;
	return true;
}
void point_operator::MultiplyPointByConstant(Point A[3], float k, Point point[3])
{
	for(int i=0; i<3; i++)
		point[i] = A[i] * k;
}
void point_operator::Normal(Point pt[3][3], Point normal[3])
{
	Point AMinusB[3], CMinusB[3];
	SubtractTwoPoints(pt[0], pt[1],AMinusB);
	SubtractTwoPoints(pt[2], pt[1],CMinusB);
	CrossProduct(AMinusB, CMinusB, normal);
}
Point point_operator::DistanceValueOfPlaneEquation(Point normal[3], Point pt[3][3])
{
	MultiplyPointByConstant(normal, -1, normal);
	return DotProduct(normal, pt[1]);
}
bool point_operator::PointIsWithinTwoPoints(Point checkpt[3], Point pt0[3], Point pt1[3])
{
	Point u;
	int n=0;
	while(n < 3) {
		if( (pt1[n] - pt0[n]) != 0) {
			u = (checkpt[n] - pt0[n]) / (pt1[n] - pt0[n]);
			if( u < 1.0 && u > 0.0)
				if( AlmostEqual2sComplement(checkpt[0],(pt0[0] + u*(pt1[0] - pt0[0])),1) && 
					AlmostEqual2sComplement(checkpt[1],(pt0[1] + u*(pt1[1] - pt0[1])),1) && 
					AlmostEqual2sComplement(checkpt[2],(pt0[2] + u*(pt1[2] - pt0[2])),1))
					return true;
			return false;
		}
		n++;
	}
	return false;
}
bool point_operator::PointIsInsideTriangle(float tri_pts[3][3], float check_pt[3])
{
	// Check if point is at any vertex of triangle or edge
	for( int i=0; i<3; i++) {
		if( AreTwoPointsEqual(check_pt, tri_pts[i])) {
			return true;
		}
		if( PointIsWithinTwoPoints(check_pt, tri_pts[i], tri_pts[(i+1)%3]))
			return true;
	}
	if( SameSide(check_pt, tri_pts[0], tri_pts[1], tri_pts[2]) &&
		SameSide(check_pt, tri_pts[1], tri_pts[0], tri_pts[2]) &&
		SameSide(check_pt, tri_pts[2], tri_pts[0], tri_pts[1]) )
		return true;
	else
		return false;
	//// Compute vectors
	//float a[3], b[3], c[3], tmp[3];
	//SubtractTwoPoints(tri_pts[2], tri_pts[0], a);
	//SubtractTwoPoints(tri_pts[1], tri_pts[0], b);
	//SubtractTwoPoints(check_pt, tri_pts[0], c);

	//// Compute dot products
	//float dot[5];
	//dot[0] = DotProduct(a,a);
	//dot[1] = DotProduct(a,b);
	//dot[2] = DotProduct(a,c);
	//dot[3] = DotProduct(b,b);
	//dot[4] = DotProduct(b,c);

	//// Compute Barycentric coordinates
	//float invDenom = 1.0 /(dot[0] * dot[3] - dot[1] * dot[1]);
	//float u = (dot[3] * dot[2] - dot[1] * dot[4]) * invDenom;
	//float v = (dot[0] * dot[4] - dot[1] * dot[2]) * invDenom;

	//// Check if point is in triangle. On edge or within the border.
	//if((u > 0) && (v > 0) && ((u + v) < 1)) {
	//	return true;
	//}
	//return false;
}
// Function taken from Moller97
bool point_operator::PointInTriangle(Point V0[3],Point U0[3],Point U1[3], Point U2[3])           \
{                                           \
  float a,b,c,d0,d1,d2;                     \
  /* is T1 completly inside T2? */          \
  /* check if V0 is inside tri(U0,U1,U2) */ \
  a=U1[1]-U0[1];                          \
  b=-(U1[0]-U0[0]);                       \
  c=-a*U0[0]-b*U0[1];                     \
  d0=a*V0[0]+b*V0[1]+c;                   \
                                            \
  a=U2[1]-U1[1];                          \
  b=-(U2[0]-U1[0]);                       \
  c=-a*U1[0]-b*U1[1];                     \
  d1=a*V0[0]+b*V0[1]+c;                   \
                                            \
  a=U0[1]-U2[1];                          \
  b=-(U0[0]-U2[0]);                       \
  c=-a*U2[0]-b*U2[1];                     \
  d2=a*V0[0]+b*V0[1]+c;                   \
  if(d0*d1>0.0)                             \
  {                                         \
    if(d0*d2>0.0) return 1;                 \
  }    
  return 0;\
}
// Usable AlmostEqual function
bool point_operator::AlmostEqual2sComplement(float A, float B, int maxUlps)
{
	
	if( abs(A-B) < 0.000001)
		return true;
	else
		return false;
		
	/*
    // Make sure maxUlps is non-negative and small enough that the
    // default NAN won't compare as equal to anything.
    assert(maxUlps > 0 && maxUlps < 4 * 1024 * 1024);
    int aInt = *(int*)&A;
    // Make aInt lexicographically ordered as a twos-complement int
    if (aInt < 0)
        aInt = 0x80000000 - aInt;
    // Make bInt lexicographically ordered as a twos-complement int
    int bInt = *(int*)&B;
    if (bInt < 0)
        bInt = 0x80000000 - bInt;
    int intDiff = abs(aInt - bInt);
    if (intDiff <= maxUlps)
        return true;
    return false;
	*/
}
bool point_operator::BothPointsOnTheSameEdge(float p[3], float q[3], float pts[3][3])
{
	int i,j;
	i=0; j=1;
	while(i<3){
		if( PointIsWithinTwoPoints(p, pts[i],pts[j]) && PointIsWithinTwoPoints(q, pts[i],pts[j]))
			return true;
		i++; j++;
		if(j==3) j=0;
	}
	return false;
}
bool point_operator::FindSegmentEdgeIntersection(float p[3], float q[3], float r[3], float s[3], float pt[3])
{
	float sMinusr[3], qMinusp[3], rMinusp[3], Cross[3];
	float tmp[3];

	SubtractTwoPoints(q, p, qMinusp);	// q-p
	SubtractTwoPoints(s, r, sMinusr);	// s-r

	CrossProduct( qMinusp, sMinusr, Cross);
	float zero[3] = {0.0,0.0,0.0};
	if( AreTwoPointsEqual(Cross, zero))	// parallel
	{
		if( AreTwoPointsEqual(p,s) || AreTwoPointsEqual(q,s)) {
			SetPointEqualToPoint(s, pt);
			return true;
		}
		if( AreTwoPointsEqual(p,r) || AreTwoPointsEqual(q,r)) {
			SetPointEqualToPoint(r, pt);
			return true;
		}
		return false;
	}
	
	SubtractTwoPoints(r, p, rMinusp);// r-p
	CrossProduct( rMinusp, sMinusr, tmp);
	int i=0; 
	while( i<3){
		if( Cross[i] != 0.0)
			break;
		i++;
	}
	float t = tmp[i]/Cross[i];

	CrossProduct( rMinusp, qMinusp, tmp);
	i=0; 
	while( i<3) {
		if( Cross[i] != 0.0)
			break;
		i++;
	}
	float u = tmp[i]/Cross[i];

	// Precision control
	if( AlmostEqual2sComplement(u, 1.0,1))
		u = 1.0;
	if( AlmostEqual2sComplement(u, 0.0,1))
		u = 0.0;
	if( AlmostEqual2sComplement(t, 1.0,1))
		t = 1.0;
	if( AlmostEqual2sComplement(t, 0.0,1))
		t = 0.0;

	if( u >1.0 || u <0.0 || t >1.0 || t <0.0) 
		return false;
	MultiplyPointByConstant(sMinusr, u, tmp);   // u * (B - A)
	AddTwoPoints(r, tmp, pt);				// A + u * (B - A)

	return true;
}
bool point_operator::SameSide(float a[3], float b[3] ,float c[3], float d[3])
{
	float cpt0[3], cpt1[3];
	float tmp0[3], tmp1[3], tmp2[3];
	SubtractTwoPoints(d,c, tmp0);
	SubtractTwoPoints(a,c, tmp1);
	SubtractTwoPoints(b,c, tmp2);
	CrossProduct(tmp0, tmp1, cpt0);
	CrossProduct(tmp0, tmp2, cpt1);
	if( DotProduct(cpt0, cpt1) >= 0 )
		return true;
	else
		return false;
}